#include "mymath.h"
#include "3D_model.h"
#include "math.h"
#include "mpu-6050.h"
#include "imu.h"
#include "usart.h"
#include "string.h"
#include "mymath.h"
#include "filter.h"
#include "oled.h"
#include "time.h"
#include "mytask.h" 
extern	 float rol;
extern	 float pit;
extern	 float yaw;	
extern struct myaxis g;
extern struct myaxis a;
extern struct myaxis g_d;
extern struct myaxis a_d;
extern u16 RX_CH[];
extern u8 menu;
extern u16 TX_CH[];
void instrument_display()
{
	    	/*****************获取加速度计初始校准值******************/
	 OLED_ShowString(20,20,"please stability");
	 display();
		dispaly_Clear();
		vTaskDelay(1000);
		MPU_Get_Gyroscope(&g_d.x,&g_d.y,&g_d.z);                
		MPU_Get_Accelerometer(&a_d.x,&a_d.y,&a_d.z);
/*建立模型*/
	//float cube[8][3]={{-15,-15,-15},{-15,15,-15},{15,15,-15},{15,-15,-15},{-15,-15,15},{-15,15,15},{15,15,15},{15,-15,15}};//立方体各点坐标
	//int lineid[]={1,2,2,3,3,4,4,1,5,6,6,7,7,8,8,5,8,4,7,3,6,2,5,1};//记录点之间连接顺序
	 float cube[8][3]={{-2,0,0},{5,0,0},{10,-5,0},{15,0,0},{22,0,0}};//模型各点坐标
	 float cube_bb[8][3]={{-2,0,0},{5,0,0},{10,-5,0},{15,0,0},{22,0,0}};//模型上一时间节点各点坐标
	 int lineid[]={1,2,2,3,3,4,4,5};//模型点之间连接顺序
   
   u32 geo=0;
	 float h =0;
	/*对输入得模型进行等比放大*/
//		for(int i=0;i<5;i++)
//		  for(int j=0;j<3;j++)
//					{
//					   cube[i][j]=cube[i][j]*3;
//					   cube_bb[i][j]=cube_bb[i][j]*3;
//					}

	while(menu==0)
	{
		 
		 	MPU_Get_Gyroscope(&g.x,&g.y,&g.z);                
			MPU_Get_Accelerometer(&a.x,&a.y,&a.z);
			/*姿态解算*/
			IMU_update((float)Get_Cycle_T(0)/1000000.0f);
			TX_CH[0]=1500+pit*10;
			TX_CH[1]=1500+rol*10;
			TX_CH[2]=1500+yaw*10;
	/******************俯仰刻度*****************/		
			LCD_DrawLine(64-30,32+20,64-20,32+20);
			LCD_DrawLine(64-30,32+10,64-20,32+10);
			LCD_DrawLine(64-30,32   ,64-15,32);
			LCD_DrawLine(64-30,32-10,64-20,32-10);
			LCD_DrawLine(64-30,32-20,64-20,32-20);
			
			LCD_DrawLine(64+50,32+20,64+40,32+20);
			LCD_DrawLine(64+50,32+10,64+40,32+10);
			LCD_DrawLine(64+50,32   ,64+35,32);
			LCD_DrawLine(64+50,32-10,64+40,32-10);
			LCD_DrawLine(64+50,32-20,64+40,32-20);
	/******************俯仰横滚游标*****************/		
			for(int i=0;i<5;i++) move(cube[i],0,rol/3,0);//俯仰
			for(int i=0;i<5;i++)rotate(cube[i],0,0,pit);//旋转每个点
			for(int i=0;i<8;i+=2)//绘制模型顺序连接每个点
				LCD_DrawLine(64+cube[lineid[i]-1][0],32+cube[lineid[i]-1][1],64+cube[lineid[i+1]-1][0],32+cube[lineid[i+1]-1][1]);

			if((yaw-10)<0)//航向补码
				geo=(yaw+350)*2;
			else
				 geo=(yaw-10)*2;
			h++;
			//LPF_1((float)Get_Cycle_T(0)/1000000.0f,100.0f,(float) RX_CH[0], &h) ;
			memcpy(cube,cube_bb,sizeof(cube));
			/*******************高度显示***************************/	
			OLED_ShowNum(0,(u16)h/10%14, h/140+3,3,12);
			OLED_ShowNum(0,14+(u16)h/10%14,h/140+2,3,12);
			OLED_ShowNum(0,28+(u16)h/10%14,h/140+1,3,12);
			OLED_ShowNum(0,42+(u16)h/10%14,h/140,3,12);
			dispaly_fill_rect(0,56,20,63,0);
			dispaly_fill_rect(0,0,20,7,0);
			LCD_DrawRectangle(0, 32-5,20,32+5);
			/*******************航向显示***************************/	
			OLED_ShowNum(26+geo%20,2,(geo/20+3)*10%360,3,12);
			OLED_ShowNum(46+geo%20,2,(geo/20+2)*10%360,3,12);
			OLED_ShowNum(66+geo%20,2,(geo/20+1)*10%360,3,12);
			OLED_ShowNum(86+geo%20,2,(geo/20+0)*10%360,3,12);
			dispaly_fill_rect(26,0,26+18,10,0);
			dispaly_fill_rect(110,0,110+18,10,0);
			LCD_DrawRectangle(64,0,64+20,10);
			
			LCD_DrawLine(20,0,20,63);
			display();
			dispaly_Clear();
		}
    
}
float* matconv(float* a,float b[3][3]){//计算矩阵乘法
        float res[3];
        for(int i=0;i<3;i++)
           res[i]=b[i][0]*a[0]+b[i][1]*a[1]+b[i][2]*a[2];
        for(int i=0;i<3;i++)
           a[i]=res[i];
        return a;
}

void rotate(float* obj,float x,float y,float z){//绕（0，0）点旋转
		x*=RAD_PER_DEG;y*=RAD_PER_DEG;z*=RAD_PER_DEG;//角度转化成弧度
		float rz[3][3]={{cos(z),-sin(z),0},{sin(z),cos(z),0},{0,0,1}};
		float ry[3][3]={{1,0,0},{0,cos(y),-sin(y)},{0,sin(y),cos(y)}};
		float rx[3][3]={{cos(x),0,sin(x)},{0,1,0},{-sin(x),0,cos(x)}};
		matconv(matconv(matconv(obj,rz),ry),rx);
}

void move(float* obj,float x,float y,float z){
		*(obj)=*(obj)+x;
		*(obj+1)=*(obj+1)+y;
		*(obj+2)=*(obj+2)+z;
}
  